/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMMTools
* @author     Christian Mandery
* @copyright  2015 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_NloptConverter_H_
#define __MMM_NloptConverter_H_

#include <nlopt.hpp>

#include <MMM/Motion/Legacy/Converter/MarkerBasedConverter.h>
#include <VirtualRobot/VirtualRobotCommon.h>

#include "NloptConverterImportExport.h"

namespace MMM
{

/*!
    \brief A converter using NLopt to convert Vicon marker motions to the MMM format.
*/
class NloptConverter_IMPORT_EXPORT NloptConverter : public MarkerBasedConverter
{
public:
    enum OptimizationStrategy {
        FRAMEWISE_LOCAL,  /* Optimize each frame by itself, do not consider inter-frame constraints/objective */
        WHOLE_MOTION /* Optimize all frames together, using a relative representation for position/rotation changes and absolute representation for joint angles */
    };

    NloptConverter(const std::string &name = "NloptConverter");

    virtual bool setup(ModelPtr inputModel, AbstractMotionPtr inputMotion, ModelPtr outputModel);

    virtual AbstractMotionPtr convertMotion();

    virtual AbstractMotionPtr initializeStepwiseConversion();
    virtual bool convertMotionStep(AbstractMotionPtr currentOutput, bool increment=true);

protected:
    virtual bool _setup(rapidxml::xml_node<char>* rootTag);

private:
    AbstractMotionPtr convertMotionWholeMotion();

    bool readModelConfigXml(rapidxml::xml_node<char>* modelNode);
    bool readOptimizationConfigXml(rapidxml::xml_node<char>* optimizationNode);

    LegacyMarkerDataPtr getInputMarkerData(unsigned int frameNum) const;

    void setOptimizationBoundsFramewiseLocal(nlopt::opt& optimizer) const;
    void setOptimizationBoundsWholeMotion(nlopt::opt& optimizer) const;

    static double objectiveFunctionWrapperStatic(const std::vector<double> &configuration, std::vector<double> &grad, void *data);
    double objectiveFunctionWrapper(const std::vector<double> &configuration, std::vector<double> &grad);

    double objectiveFunctionFramewiseLocal(const std::vector<double> &configuration, std::vector<double> &grad, bool debugOutput);
    double objectiveFunctionWholeMotion(const std::vector<double> &configuration, std::vector<double> &grad, bool debugOutput);
    void setOutputModelConfiguration(const std::vector<double> &configuration);
    void calculateMarkerDistancesAverageMaximum(LegacyMarkerDataPtr markerFrame, double &avgDistance, double &maxDistance) const;
    double calculateMarkerDistancesSquaresSum(LegacyMarkerDataPtr markerFrame) const;

    OptimizationStrategy optimizationStrategy;
    nlopt::algorithm nloptAlgorithm;
    unsigned int maxFrames, objectiveFunctionDebugNFrames, objectiveFunctionEvalCounter;  // only for debugging

    std::vector<LegacyMarkerDataPtr> allFrames;  // only used for whole-motion strategy
    VirtualRobot::RobotPtr inputRobot, outputRobot;
    float markerScaleFactor;

    // The following members are not strictly necessary but used for performance optimizations
    LegacyMarkerDataPtr curFrame;
    std::map<std::string, VirtualRobot::SensorPtr> inputMarkersToOutputRobotSensors;
    std::map<std::string, float> jointValuesToSet;
};

typedef boost::shared_ptr<NloptConverter> NloptConverterPtr;

}

#endif 
